function [px, py, gx, gy] = position_7_link(z,P) 
%[px,py,gx,gy] = POSITION_7_LINK(Z,P)
% 
%FUNCTION:  This function computes the cartesian positions
%    of the CoM and tip of each link
%INPUTS: 
%
%
%OUTPUTS: 
%    px = [nLink X nTime] position of the end of each link
%    py = [nLink X nTime] position of the end of each link
%    gx = [nLink X nTime] position of the CoM of each link
%    gy = [nLink X nTime] position of the CoM of each link
% 
%NOTES:
%    This file was automatically generated by writePosition.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
m6 = P.m(6); % Link 6 mass
m7 = P.m(7); % Link 7 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
l6 = P.l(6); % Link 6 length
l7 = P.l(7); % Link 7 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
I6 = P.I(6); % Link 6 moment of inertia about its center of mass
I7 = P.I(7); % Link 7 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint
d6 = P.d(6); % Link 6 distance between center of mass and parent joint
d7 = P.d(7); % Link 7 distance between center of mass and parent joint

th1 = z(1,:); 
th2 = z(2,:); 
th3 = z(3,:); 
th4 = z(4,:); 
th5 = z(5,:); 
th6 = z(6,:); 
th7 = z(7,:); 

nTime = length(th1); 
px = zeros(7,nTime);
py = zeros(7,nTime);
gx = zeros(7,nTime);
gy = zeros(7,nTime);

px(1,:) = l1.*cos(th1);
py(1,:) = l1.*sin(th1);
gx(1,:) = d1.*cos(th1);
gy(1,:) = d1.*sin(th1);

px(2,:) = l1.*cos(th1) + l2.*cos(th2);
py(2,:) = l1.*sin(th1) + l2.*sin(th2);
gx(2,:) = d2.*cos(th2) + l1.*cos(th1);
gy(2,:) = d2.*sin(th2) + l1.*sin(th1);

px(3,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
py(3,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);
gx(3,:) = d3.*cos(th3) + l1.*cos(th1) + l2.*cos(th2);
gy(3,:) = d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2);

px(4,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
py(4,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);
gx(4,:) = d4.*cos(th4) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3);
gy(4,:) = d4.*sin(th4) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3);

px(5,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
py(5,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);
gx(5,:) = d5.*cos(th5) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4);
gy(5,:) = d5.*sin(th5) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4);

px(6,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6);
py(6,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6);
gx(6,:) = d6.*cos(th6) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5);
gy(6,:) = d6.*sin(th6) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5);

px(7,:) = l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6) + l7.*cos(th7);
py(7,:) = l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6) + l7.*sin(th7);
gx(7,:) = d7.*cos(th7) + l1.*cos(th1) + l2.*cos(th2) + l3.*cos(th3) + l4.*cos(th4) + l5.*cos(th5) + l6.*cos(th6);
gy(7,:) = d7.*sin(th7) + l1.*sin(th1) + l2.*sin(th2) + l3.*sin(th3) + l4.*sin(th4) + l5.*sin(th5) + l6.*sin(th6);


end 
